#include "Compass.h"

//将正数转成负数，将负数转成正数
#include <stdio.h>

//JY-RM3100 帧解析索引
uint8_t MPU_INDEX_END,MPU_INDEX_BEGIN,MPU_INDEX_START;
//JY-RM3100 帧解析标志
char *Start = {"Magx"};
char *Begin = {":"};
char *End = {","};
unsigned char StartMeasure[] = {0x68,0x04,0x00,0x04,0x08,'\r'};

//uint8_t buf[2048]; //定义全局变量
char StractDes[] = {":"};
struct _MPU_DATA
	{
		int  Magx;
		int	 Magy;
		int	 Magz;
		float	 Yaw;
	}_MPU_Date;




int changeSymbol(int num)
{
  if (num < 0)
	{
		num = ~(num - 1);
	}
	else if (num >0)
	{
		num = ~num + 1;
	}
	return num;
}

int CompassStartMeasure(void)
	
{
	static int8_t num;
	num++;
	if(num>5)
	{
		num = 0;
		Uart4Send(StartMeasure,MyStrlen(StartMeasure));
	}
	else
		return 0;
	
	return 1;
}


void CompassdAnalyse(char *buf)
{
	char pitchbuf[3],rollbuf[3],headbuf[3];
	uint16_t  pitchSymbol;
	uint16_t 	rollSymbol;
	uint16_t 	headSymbol;

	
	
	//get pitch 俯仰角
	pitchbuf[0] = buf[4];
	pitchbuf[1] = buf[5];
	pitchbuf[2] = buf[6];
	pitchSymbol = (buf[4] & 0xf0) >> 4;
	
	////////////////////////////
	if(pitchSymbol == 1) //负
	OsdRefresh_Family[_Pitch].Module_addr[0] = osd_Icon_jia;
	else //正
	OsdRefresh_Family[_Pitch].Module_addr[0] = osd_Icon_jian;

	OsdRefresh_Family[_Pitch].Module_addr[1] = (pitchbuf[0] 	& 0x0f)     + 0x30;
	OsdRefresh_Family[_Pitch].Module_addr[2] = ((pitchbuf[1]  & 0xf0)>>4) + 0x30;
	OsdRefresh_Family[_Pitch].Module_addr[3] = (pitchbuf[1]   & 0x0f)     + 0x30;
	OsdRefresh_Family[_Pitch].Module_addr[4] = osd_Icon_point;
	OsdRefresh_Family[_Pitch].Module_addr[5] = ((pitchbuf[2]  & 0xf0)>>4) + 0x30;
	
	//get head 航向角
	headbuf[0] = buf[10];
	headbuf[1] = buf[11];
	headbuf[2] = buf[12];
	headSymbol = (buf[10] & 0xf0)>> 4;

	////////////////////////////
//	if(headSymbol == 1)
//	OsdRefresh_Family[_Position].Module_addr[0] = osd_Icon_jia;
//	else
//	OsdRefresh_Family[_Position].Module_addr[0] = osd_Icon_jian;

	OsdRefresh_Family[_Position].Module_addr[0] = osd_NONE;
	OsdRefresh_Family[_Position].Module_addr[1] = (headbuf[0] & 0x0f) + 0x30;
	OsdRefresh_Family[_Position].Module_addr[2] = ((headbuf[1] & 0xf0)>>4) + 0x30;
	OsdRefresh_Family[_Position].Module_addr[3] = (headbuf[1] & 0x0f) + 0x30;
	OsdRefresh_Family[_Position].Module_addr[4] = osd_Icon_point;
	OsdRefresh_Family[_Position].Module_addr[5] = ((headbuf[2] & 0xf0)>>4) + 0x30;


	OsdRefresh_Family[_Pitch].Refresh_Flag = Refresh_yes;
	OsdRefresh_Family[_Position].Refresh_Flag = Refresh_yes;
}



_Bool  Analyse_JY3100(uint8_t *BUFF)
{
	uint8_t j,t;
	uint8_t DataNum = 0,RetNum = 0;
	_Bool Flag_Start = FALSE;
	static _Bool Flag_ret = FALSE; 
	char buf_char[16];
	int Magx,Magy,Magz,Yaw;
	static uint8_t Local = 0;
	uint8_t addNum,Newlen;
	
	memset(buf_char,0,16);
	
	for(j = Local; j < UART4_BUFLEN; j++)
	{
		//找到了Magx ,但是数据不完整，清空之前接收的结果
		if(BUFF[j] == 0x0d && RetNum < 3)
		{
			Flag_ret = FALSE;
			RetNum = 0;
			DataNum = 0;
			memset(buf_char,0,16);
			
			Magx = 0; 
			Magy = 0;
			Magz = 0;
			Yaw = 0;
		}
		
		//数据接收、保存
		if(Flag_Start == TRUE)
		{
			//结束
			if(FindString(End,&MPU_INDEX_END,1,BUFF[j]) || (BUFF[j] == 0x0d && RetNum == 3))
			{
				Flag_ret = FALSE;
				RetNum ++;
				DataNum = 0;
				switch(RetNum)
				{
					case 1: 
						Magx = atoi((const char *)buf_char); 
						break;
					case 2: 
						Magy = atoi((const char *)buf_char); 
						break;
					case 3: 
						Magz = atoi((const char *)buf_char); 
						break;
					case 4: 
						Yaw = atoi((const char *)buf_char); 
						break;
					default:
						break;					
				}
				memset(buf_char,0,16);
			}
			
			//接收
			if(Flag_ret == TRUE)
			{
				buf_char[DataNum] = BUFF[j];
				DataNum ++;
			}
			
			//开始
			if(FindString(Begin,&MPU_INDEX_BEGIN,1,BUFF[j]))
			{
				Flag_ret = TRUE;
			}
			
			//完成
			if(RetNum >= 4)
			{
				_MPU_Date.Magx = Magx;
				_MPU_Date.Magy = Magy;
				_MPU_Date.Magz = Magz;
				_MPU_Date.Yaw = Yaw;
				
				Local = j;
				Newlen = 0;
				Local = 0;
				return 1;
			}
		}
		//帧头
		if(FindString(Start,&MPU_INDEX_START,strlen((const char *)Start),BUFF[j]))
		{
			Flag_Start = TRUE;
		}
	}
	
	Local = 0;
	return 0;
}

//找字符串
uint8_t FindString(const char * pBuf, uint8_t *pindex, uint8_t compare_len, char new_byte)
{
	if(pBuf[*pindex] == new_byte)
	{
		(*pindex)++;
	}
	else
	{
		(*pindex) = 0;
	}
	if((*pindex)>=compare_len)
	{
		(*pindex) = 0;
		return 1;
	}
	else
		return 0;
}